#include "setservoparameter.h"
#include "ui_setservoparameter.h"
#include "mccontrollerinterpreter.h"

SetServoParameter::SetServoParameter(McControllerInterpreter* mcInterpreterIn, QWidget *parent) :
    QWidget(parent),
    ui(new Ui::SetServoParameter)
{
    ui->setupUi(this);
    mcInterpreter = mcInterpreterIn;
}

SetServoParameter::~SetServoParameter()
{
    delete ui;
}

int SetServoParameter::getServoParameter()
{
    QVector<int> checkBoxList;
    QVector<unsigned long long> posKpp;
    QVector<unsigned long long> posFeedFrontFilter;
    QVector<unsigned long long> velKpp;
    QVector<unsigned long long> velIntegration;
    QVector<unsigned long long> velFeedBackFilter;

    checkBoxList.resize(9);
    posKpp.resize(9);
    posFeedFrontFilter.resize(9);
    velKpp.resize(9);
    velIntegration.resize(9);
    velFeedBackFilter.resize(9);

    checkBoxList[0] = ui->checkBox1->isChecked();
    checkBoxList[1] = ui->checkBox2->isChecked();
    checkBoxList[2] = ui->checkBox3->isChecked();
    checkBoxList[3] = ui->checkBox4->isChecked();
    checkBoxList[4] = ui->checkBox5->isChecked();
    checkBoxList[5] = ui->checkBox6->isChecked();
    checkBoxList[6] = ui->checkBox7->isChecked();
    checkBoxList[7] = ui->checkBox8->isChecked();

    posKpp[0] = ui->lineEdit1->text().toULongLong();
    posKpp[1] = ui->lineEdit6->text().toULongLong();
    posKpp[2] = ui->lineEdit11->text().toULongLong();
    posKpp[3] = ui->lineEdit16->text().toULongLong();
    posKpp[4] = ui->lineEdit21->text().toULongLong();
    posKpp[5] = ui->lineEdit26->text().toULongLong();
    posKpp[6] = ui->lineEdit31->text().toULongLong();
    posKpp[7] = ui->lineEdit36->text().toULongLong();

    posFeedFrontFilter[0] = ui->lineEdit2->text().toULongLong();
    posFeedFrontFilter[1] = ui->lineEdit7->text().toULongLong();
    posFeedFrontFilter[2] = ui->lineEdit12->text().toULongLong();
    posFeedFrontFilter[3] = ui->lineEdit17->text().toULongLong();
    posFeedFrontFilter[4] = ui->lineEdit22->text().toULongLong();
    posFeedFrontFilter[5] = ui->lineEdit27->text().toULongLong();
    posFeedFrontFilter[6] = ui->lineEdit32->text().toULongLong();
    posFeedFrontFilter[7] = ui->lineEdit37->text().toULongLong();

    velKpp[0] = ui->lineEdit3->text().toULongLong();
    velKpp[1] = ui->lineEdit8->text().toULongLong();
    velKpp[2] = ui->lineEdit13->text().toULongLong();
    velKpp[3] = ui->lineEdit18->text().toULongLong();
    velKpp[4] = ui->lineEdit23->text().toULongLong();
    velKpp[5] = ui->lineEdit28->text().toULongLong();
    velKpp[6] = ui->lineEdit33->text().toULongLong();
    velKpp[7] = ui->lineEdit38->text().toULongLong();

    velIntegration[0] = ui->lineEdit4->text().toULongLong();
    velIntegration[1] = ui->lineEdit9->text().toULongLong();
    velIntegration[2] = ui->lineEdit14->text().toULongLong();
    velIntegration[3] = ui->lineEdit19->text().toULongLong();
    velIntegration[4] = ui->lineEdit24->text().toULongLong();
    velIntegration[5] = ui->lineEdit29->text().toULongLong();
    velIntegration[6] = ui->lineEdit34->text().toULongLong();
    velIntegration[7] = ui->lineEdit39->text().toULongLong();

    velFeedBackFilter[0] = ui->lineEdit5->text().toULongLong();
    velFeedBackFilter[1] = ui->lineEdit10->text().toULongLong();
    velFeedBackFilter[2] = ui->lineEdit15->text().toULongLong();
    velFeedBackFilter[3] = ui->lineEdit20->text().toULongLong();
    velFeedBackFilter[4] = ui->lineEdit25->text().toULongLong();
    velFeedBackFilter[5] = ui->lineEdit30->text().toULongLong();
    velFeedBackFilter[6] = ui->lineEdit35->text().toULongLong();
    velFeedBackFilter[7] = ui->lineEdit40->text().toULongLong();

    for (int i = 0; i < 8; i++)
    {
        if ( checkBoxList[i]==1)
        {
            mcInterpreter->servoParameter.posKpp[i] = posKpp[i];
            mcInterpreter->servoParameter.posFeedFrontFilter[i] = posFeedFrontFilter[i];
            mcInterpreter->servoParameter.velKpp[i] = velKpp[i];
            mcInterpreter->servoParameter.velIntegration[i] = velIntegration[i];
            mcInterpreter->servoParameter.velFeedBackFilter[i] = velFeedBackFilter[i];
        }
    }
}

void SetServoParameter::on_pushButton_clicked()
{
    getServoParameter();
    mcInterpreter->writeServoParameter();
}
